#include "ins.h"
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

#include "BMI088driver.h"
#include "MahonyAHRS.h"

float gyro[3], accel[3], temp;
float pitch, yaw, roll;

void INS_TASK(void *argument)
{
    while (BMI088_init())
    {
        ;
    }
    Mahony_Init(1000); 
    BMI088_read(gyro, accel, &temp);
    MahonyAHRSinit(accel[0], accel[1], accel[2], 0, 0, 0);
    for (;;)
    {
        BMI088_read(gyro, accel, &temp);
        Mahony_update(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], 0, 0, 0);
        Mahony_computeAngles(); // 角度计算

        // ekf获取姿态角度函数
        pitch = getPitch(); // 获得pitch
        roll = getRoll();   // 获得roll
        yaw = getYaw();     // 获得yaw
        vTaskDelay(1);
    }
}